#include "arm.h"
                
int id5_rad_num;
int id4_rad_num;
int id3_rad_num;
int id2_rad_num;
int id1_rad_num;

void SendRadToMoto(int id,int rad)
{
  if(id >=0 && id<= 5)
  {
    if(rad>= 510 && rad <= 2490)
    {
      printf("#%03dP%04dT1000!",id,rad);
    }
  }  
}
int PointToAngle(float x_point,float y_point,float z_point,int cmd)
{
  double p,pAngle;
  double cosUnderp,cosMidp;
  double x,y,z;
  if( cmd == 0)                                      //����
  {
     x = x_point - TOPLINE;
     y = y_point;
     z = z_point;
  }
  else                                              //����
  {
    x = x_point;
    y = y_point;
    z = z_point + TOPLINE;  
  }

  z = z_point - BASEHIGH;

   
  if( x == 0 )
  {
    BaseAngle = 90.0;
  }
  else
  {
    BaseAngle = 180 + ( ( ( atan(y/-x) )/PI )*180 );
  }

   
   p = sqrt( sq(x) + sq(z) + sq(y) );
   x = sqrt( sq(x) + sq(y) );
   pAngle = 180 + ((atan(z/-x))/PI)*180;
    
   if( JudgeTriangle(p) == -1 )
   {
      return -1; 
   }
   else
   {
     cosMidp = ( sq(MIDLINE) + sq(p) - sq(UNDERLINE) )/( 2*p*MIDLINE );
     cosUnderp = ( sq(UNDERLINE) + sq(p) - sq(MIDLINE) )/( 2*p*UNDERLINE );
  
     UnderAngle = pAngle - (acos(cosUnderp)/PI)*180;
     MidAngle  = (acos(cosUnderp)/PI)*180 + (acos(cosMidp)/PI)*180;
  
     if( cmd == 0)
     {
        TopAngle = 180 - MidAngle - UnderAngle;
     }
     else
     {
        TopAngle = 270 - MidAngle - UnderAngle;
     }
   }
   return 0;
}
int JudgeTriangle(float p)
{
  if( p > ( UNDERLINE -  MIDLINE ) && p <= ( UNDERLINE +  MIDLINE ) )
  {
    return 0;
  }
  else
  {
    return -1;
  }
}
int AngleToMotoRad(void)
{
  if( BaseAngle <= 86.85 || BaseAngle >= 354.15)
  {
    return -1;  
  }
  if( UnderAngle < 0 || UnderAngle > 180 )
  {
    return -1;  
  }
  if( TopAngle < -133.56 || TopAngle > 133.65 )
  {
    return -1;
  }
  id5_rad_num = int(510 + ( BaseAngle - 86.85 )/0.135);

  id4_rad_num = int(833 + ( UnderAngle/0.135 ));

  id3_rad_num = int(1500 - ( MidAngle/0.135 ));

  if( TopAngle > 0 )
  {
    id2_rad_num = int(1500 - (TopAngle/0.135));
  }
  else
  {
    id2_rad_num = int(1500 + (-TopAngle/0.135));
  }
  return 0;
}
void ArmToCatchPoint(void)
{
  SendRadToMoto(0,2000);
  SendRadToMoto(5,id5_rad_num);
  SendRadToMoto(1,id1_rad_num);
  SendRadToMoto(2,id2_rad_num);
  SendRadToMoto(3,id3_rad_num);
  SendRadToMoto(4,id4_rad_num);
}
void ArmReset(void)
{
  printf("{G0000#001P1550T1000!#002P1500T1000!#003P1500T1000!#004P1500T1000!#005P1200T1000!}");
  id5_rad_num = 1200;
  id4_rad_num = 1500;
  id3_rad_num = 1500;  
  id2_rad_num = 1500;
  id1_rad_num = 1550;
}
void ArmPut(void)
{
  printf("{G0000#001P1550T1000!#002P1500T1000!#003P0900T1000!#004P1500T1000!#005P1867T1000!}");
  id5_rad_num = 1867;
  id4_rad_num = 1500;
  id3_rad_num = 900;
  id2_rad_num = 1500;
  id1_rad_num = 1550;
  printf("#000P2000T1000!");
}
double sq(double x)
{
	double result;
	result = pow(x,2);
	
	return result;
}